#ifndef __motor_control
#define __motor_control
#include "pid.h"
#include "filter.h"
#include "Can_Control.h"
#include "remote_control.h"
#include "GO_motor_control.h"
#include "user_lib.h"
#include "Self_control.h"
#include "cmsis_os.h"
#include "RGB.h"
#include "main.h"
#include "judge_potocol.h"
//注意注释掉的东西

typedef struct
{
	PidTypedef Speed_Pid_set;            //速度环PID设置
	PidTypedef Angle_Pid_set;            //角度环PID设置
	
	const motor_measure_t *Now_Motor_Data;      //电机的参数
	
	float Motor_Ecd;                     //电机的角度
	float Angle_Out;                     //电机角度环输出
	float Speed_Out;                     //电机速度环输出
	
}JointTypedef;

typedef struct
{
    PidTypedef Speed_Pid_set;            //速度环PID设置
    
    const motor_measure_t *Now_Motor_Data;      //电机的参数
    float WheelSpeed;
    PidTypedef Pid_Chassis;              //初始化底盘电机的pid
    
    int16_t Chassis_3508_Current;        //底盘赋值电流
}ChassisTypedef;



typedef struct
{
	
	float Va;
	float Vd;
	float Vw;
	float Vs;
	
	float speed_w_z;
	
	float set_x;
	float set_y;
	
}key_chassis;

extern judge_info_t 	judge_sensor_info;


void mouse ();
void mouse_1 ();
void chassis_key(void);   
void max_speed(void);			//控制加减速
void max_speed_1(void);	//鼠标控制底盘加减速
float *get_joint_current_angle(void);
void  Read_joint_control(int i);
void  Read_joint_control_1(int i);//图传
void motor_joint_control(float joint1,float joint2,float joint3,float joint4,float joint5,float joint6);
void ChassisMove_1(int SPEED_X, int SPEED_Y, int SPEED_W_Z);
void ChassisMove(int SPEED_X , int SPEED_Y , int SPEED_W_Z);
int LIMIT_3508(int Speed_3508,int Speed_MAX);
void Chassis_Stop(void);
void Chassis_Control_Task();
void Chassis_Control_Task_1();//图传
/**
  * @brief   读取底盘电机数据
  * @param   none
  * @retval  电机各项数据
  **/
void get_chassis_data(void);
/**
 * @brief 无刷电机运行圈数判断
 * @param none
 * @retval none
 */
void foc_count_judge();
/**
 * @brief GO-M8010-6报文初始化
 * @param none
 * @retval none
 */
void GO_M8010_6_Init();
/**
  * @param 拾取银矿石
  * @in 当前臂的六个角度
  * @warning 使用前先将臂初始化
  **/
void Capture_Mineral();
/**
 * @brief 低通滤波（缓启缓停）
 * @param[in]  PID输出的电流值
 * @param[out] 滤波后的值
 */
float pid_out_flr(float in,int i);
void Joint_Sharp_Tuning_Init(int i);
/*******************自写代码**************************/
void read_unitee(void);
void motor_joint_control_1(float joint1, float joint2, float joint3);
void motor_joint_control_2(float joint1, float joint2);
void elevator(float joint1);
void elevator_control_ADD(void);//控制机械臂升降,左右摇摆，宇树,累加值控制
void wrist_control(void);//控制机械臂2006电机
void elevator_3508(void);//底盘移动同时控制电梯
void key_control_1(void);
void key_control_2(void);//键盘控制
void Self_control_(void);//自定义控制器控制
void Joint_Init(void);
void mouse_Init(void);//鼠标值初始化
void air_pump(void);//气泵
void steer_control(void);//舵机控制
#endif
